#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "pid.h"	
#include "usart.h"
#include "locate_task.h"
#include "uart.h"
#include "timer.h"

#if 0
	#include "tim.h"
	#include "can_motor_user.h"
	uint8_t shoot_pull_flag = 0;			//拉弹簧标志位		(点动式)
	uint8_t shoot_refresh_poi_flag = 0;		//让电机归位标志位	(按压式)
	uint8_t shoot_servo_lock_flag = 0;		//舵机勾着 			(点动式)
	uint8_t shoot_servo_unlock_flag = 0;	//舵机释放			(点动式)
#endif 

extern struct keys key[4];
extern uint8_t x_signal;
extern uint8_t x_h ;
extern uint8_t x_l;
extern uint8_t y_signal;
extern uint8_t y_h ;
extern uint8_t y_l;
extern uint8_t angle_signal;
extern uint8_t angle_h;
extern uint8_t angle_l;

float X;
float Y;
float angle;

pid_t z_pid = {0};

float Z;

//用于自瞄的PID

float  EZ_pid(float angle)
{
	float out;
	
	out = pid_calc(&z_pid,-angle,0);
	return out;
}

//获取雷达位置数据
void get_position(void)
{
		X = (x_h << 8) | x_l;   			//车相对于轮框的X坐标
		Y = (y_h << 8) | y_l;				//车相对于轮框的Y坐标
	
		uint16_t locateang91 = ( ((uint16_t)angle_h) << 8) | angle_l;
		angle = (float)locateang91 / 100;   //车相对于轮框的angle坐标
		
		if(!x_signal) X=-X;					
		if(!y_signal) Y=-Y;
		if(!angle_signal) angle=-angle;
}



void LocateTask(void *argument);
osThreadId_t	LocateTaskHandle;		

const osThreadAttr_t LocateTask_attributes = {
  .name = "LocateTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,	
};


void locate_task_init(void)
{
	uint32_t maxout = 5;
	uint32_t Imax = 1;
	float deadzone  = 0.01;
	float p = 0.6;
	float i = 0.0;
	float d  = 2;
	float f = 0;
	PID_struct_init(&z_pid,POSITION_PID,D_FIRST_DISEN,maxout,Imax,deadzone,p,i,d,f);
	LocateTaskHandle = osThreadNew (LocateTask , NULL, &LocateTask_attributes  );
	
	
}






void LocateTask(void *argument)
{

	
	
	for(;;)
	{
		
		if(key[0].short_key == 1)   //xbox_memu 三条杠,松发式
		{
			uint8_t start_babadeleida[2] = {0x55,0x91};
			HAL_UART_Transmit_IT(&huart3,start_babadeleida,2);
			key[0].short_key =0 ;
		}
		
		get_position();
		Z = EZ_pid(angle);
		osDelay(1);
	}


}

